#include "../bsp.h"

#ifdef XPAR_XAXIVDMA_NUM_INSTANCES

#if defined (__MICROBLAZE__)
	#define DDR_BASEADDR XPAR_MICROBLAZE_DCACHE_BASEADDR
//	#define DDR_BASEADDR 0x80000000
#else
	#define DDR_BASEADDR XPAR_DDR_MEM_BASEADDR
#endif

#define FRAME_BUFFER_BASE_ADDR  	(DDR_BASEADDR + (0x20000000))

#define FRAME_BUFFER_SIZE0      0x2000000    //0x2000000 for max 4KW RGB888 8bpc
//#define FRAME_BUFFER_SIZE0      0x600000    //0x600000 for max 1080p RGB888 8bpc

#define FRAME_BUFFER_1          FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*1)
#define FRAME_BUFFER_2          FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*2)
#define FRAME_BUFFER_3          FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*3)
#define FRAME_BUFFER_4          FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*4)
#define FRAME_BUFFER_5          FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*5)
#define FRAME_BUFFER_6          FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*6)
#define FRAME_BUFFER_7          FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*7)
#define FRAME_BUFFER_8          FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*8)
#define FRAME_BUFFER_9          FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*9)
#define FRAME_BUFFER_10         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*10)
#define FRAME_BUFFER_11         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*11)
#define FRAME_BUFFER_12         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*12)
#define FRAME_BUFFER_13         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*13)
#define FRAME_BUFFER_14         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*14)
#define FRAME_BUFFER_15         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*15)
#define FRAME_BUFFER_16         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*16)
#define FRAME_BUFFER_17         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*17)
#define FRAME_BUFFER_18         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*18)
#define FRAME_BUFFER_19         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*19)
#define FRAME_BUFFER_20         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*20)
#define FRAME_BUFFER_21         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*21)
#define FRAME_BUFFER_22         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*22)
#define FRAME_BUFFER_23         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*23)
#define FRAME_BUFFER_24         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*24)
#define FRAME_BUFFER_25         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*25)
#define FRAME_BUFFER_26         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*26)
#define FRAME_BUFFER_27         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*27)
#define FRAME_BUFFER_28         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*28)
#define FRAME_BUFFER_29         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*29)
#define FRAME_BUFFER_30         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*30)
#define FRAME_BUFFER_31         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*31)
#define FRAME_BUFFER_32         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*32)
#define FRAME_BUFFER_33         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*33)
#define FRAME_BUFFER_34         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*34)
#define FRAME_BUFFER_35         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*35)
#define FRAME_BUFFER_36         FRAME_BUFFER_BASE_ADDR + (FRAME_BUFFER_SIZE0*36)
extern u8 current_ch;


#if defined (UDP_VIDEO)

VdmaChannel VdmaChannels[NUM_CHANNELS] =
{
#if (NUM_CHANNELS >= 1U)
    {
    	.channel_ID = 1,
        .DeviceID = XPAR_AXIVDMA_0_DEVICE_ID,
		.S2MM_INTR_ID = XPAR_PROCESSOR_SUBSYSTEM_MICROBLAZE_0_AXI_INTC_TPG_CHANNEL_0_AXI_VDMA_0_S2MM_INTROUT_INTR,
        .FrameBuffers = {FRAME_BUFFER_13, FRAME_BUFFER_14, FRAME_BUFFER_15},
		.UDP_IMG_PACKEG_SIZE = 1200,
		.VIDEO_FLAG = SEND_PIC,
	    .send_pic_start = 0,
	    .send_video_start = 0,
        .Stride = 1920,
        .Width = 1920,
        .Height = 1080
    },
#endif
#if (NUM_CHANNELS >= 2U)
	{
		.channel_ID = 2,
		.DeviceID = XPAR_AXIVDMA_1_DEVICE_ID,
		.S2MM_INTR_ID = XPAR_PROCESSOR_SUBSYSTEM_MICROBLAZE_0_AXI_INTC_TPG_CHANNEL_0_AXI_VDMA_1_S2MM_INTROUT_INTR,
		.FrameBuffers = {FRAME_BUFFER_16, FRAME_BUFFER_17, FRAME_BUFFER_18},
		.UDP_IMG_PACKEG_SIZE = 1200,
		.VIDEO_FLAG = SEND_VIDEO,
	    .send_pic_start = 0,
	    .send_video_start = 0,
        .Stride = 480,
        .Width = 480,
        .Height = 320
	},
#endif
#if (NUM_CHANNELS >= 3U)
	{
		.channel_ID = 3,
		.DeviceID = XPAR_AXIVDMA_2_DEVICE_ID,
		.S2MM_INTR_ID = XPAR_PROCESSOR_SUBSYSTEM_MICROBLAZE_0_AXI_INTC_TPG_CHANNEL_1_AXI_VDMA_0_S2MM_INTROUT_INTR,
		.FrameBuffers = {FRAME_BUFFER_19, FRAME_BUFFER_20, FRAME_BUFFER_21},
		.UDP_IMG_PACKEG_SIZE = 1200,
		.VIDEO_FLAG = SEND_PIC,
	    .send_pic_start = 0,
	    .send_video_start = 0,
        .Stride = 1920,
        .Width = 1920,
        .Height = 1080
	},
#endif
#if (NUM_CHANNELS >= 4U)
	{
		.channel_ID = 4,
		.DeviceID = XPAR_AXIVDMA_3_DEVICE_ID,
		.S2MM_INTR_ID = XPAR_PROCESSOR_SUBSYSTEM_MICROBLAZE_0_AXI_INTC_TPG_CHANNEL_1_AXI_VDMA_1_S2MM_INTROUT_INTR,
		.FrameBuffers = {FRAME_BUFFER_22, FRAME_BUFFER_23, FRAME_BUFFER_24},
		.UDP_IMG_PACKEG_SIZE = 1200,
		.VIDEO_FLAG = SEND_VIDEO,
	    .send_pic_start = 0,
	    .send_video_start = 0,
        .Stride = 480,
        .Width = 480,
        .Height = 320
	},
#endif
#if (NUM_CHANNELS >= 5U)
	{
		.channel_ID = 5,
		.DeviceID = XPAR_AXIVDMA_4_DEVICE_ID,
		.S2MM_INTR_ID = XPAR_PROCESSOR_SUBSYSTEM_MICROBLAZE_0_AXI_INTC_TPG_CHANNEL_2_AXI_VDMA_0_S2MM_INTROUT_INTR,
		.FrameBuffers = {FRAME_BUFFER_25, FRAME_BUFFER_26, FRAME_BUFFER_27},
		.UDP_IMG_PACKEG_SIZE = 1200,
		.VIDEO_FLAG = SEND_PIC,
	    .send_pic_start = 0,
	    .send_video_start = 0,
        .Stride = 1920,
        .Width = 1920,
        .Height = 1080
	},
#endif
#if (NUM_CHANNELS >= 6U)
	{
		.channel_ID = 6,
		.DeviceID = XPAR_AXIVDMA_5_DEVICE_ID,
		.S2MM_INTR_ID = XPAR_PROCESSOR_SUBSYSTEM_MICROBLAZE_0_AXI_INTC_TPG_CHANNEL_2_AXI_VDMA_1_S2MM_INTROUT_INTR,
		.FrameBuffers = {FRAME_BUFFER_28, FRAME_BUFFER_29, FRAME_BUFFER_30},
		.UDP_IMG_PACKEG_SIZE = 1200,
		.VIDEO_FLAG = SEND_VIDEO,
	    .send_pic_start = 0,
	    .send_video_start = 0,
        .Stride = 480,
        .Width = 480,
        .Height = 320
	},
#endif
#if (NUM_CHANNELS >= 7U)
	{
		.channel_ID = 7,
		.DeviceID = XPAR_AXIVDMA_6_DEVICE_ID,
		.S2MM_INTR_ID = XPAR_PROCESSOR_SUBSYSTEM_MICROBLAZE_0_AXI_INTC_TPG_CHANNEL_3_AXI_VDMA_0_S2MM_INTROUT_INTR,
		.FrameBuffers = {FRAME_BUFFER_31, FRAME_BUFFER_32, FRAME_BUFFER_33},
		.UDP_IMG_PACKEG_SIZE = 1200,
		.VIDEO_FLAG = SEND_PIC,
	    .send_pic_start = 0,
	    .send_video_start = 0,
        .Stride = 1920,
        .Width = 1920,
        .Height = 1080
	},
#endif
#if (NUM_CHANNELS >= 8U)
	{
		.channel_ID = 8,
		.DeviceID = XPAR_AXIVDMA_7_DEVICE_ID,
		.S2MM_INTR_ID = XPAR_PROCESSOR_SUBSYSTEM_MICROBLAZE_0_AXI_INTC_TPG_CHANNEL_3_AXI_VDMA_1_S2MM_INTROUT_INTR,
		.FrameBuffers = {FRAME_BUFFER_34, FRAME_BUFFER_35, FRAME_BUFFER_36},
		.UDP_IMG_PACKEG_SIZE = 1200,
		.VIDEO_FLAG = SEND_VIDEO,
	    .send_pic_start = 0,
	    .send_video_start = 0,
        .Stride = 480,
        .Width = 480,
        .Height = 320
	},
#endif
};

/*****************************************************************************/
/*
 * Call back function for write channel
 *
 * This callback handles frame count interrupts and manages the write/read
 * buffer indices for each channel.
 *
 * @param	CallbackRef is the reference pointer to the channel (VdmaChannel)
 * @param	Mask is the interrupt mask passed from the driver
 *
 * @return	None
 *
 ******************************************************************************/
static void WriteCallBack(void *CallbackRef, u32 Mask)
{
    if (CallbackRef == NULL)
    {
        DEBUG_PRINTF("WriteCallBack: CallbackRef is NULL\r\n");
        return;
    }

    VdmaChannel *Channel = (VdmaChannel *)CallbackRef;

    if (Mask & XAXIVDMA_IXR_FRMCNT_MASK)
    {
        if (Channel->WriteOneFrameEnd >= 0)
        {
            return;
        }

        int hold_rd = Channel->RdIndex;

        if (Channel->WrIndex == 2)
        {
            Channel->WrIndex = 0;
            Channel->RdIndex = 2;
        }
        else
        {
            Channel->RdIndex = Channel->WrIndex;
            Channel->WrIndex++;
        }

        int Status = XAxiVdma_StartParking(&Channel->Vdma, Channel->WrIndex, XAXIVDMA_WRITE);
        if (Status != XST_SUCCESS)
        {
            DEBUG_PRINTF("Channel %d: Failed to start parking. Status=%d\r\n",
                         Channel->DeviceID, Status);
            return;
        }

        Channel->WriteOneFrameEnd = hold_rd;

        DEBUG_PRINTF("Channel %d: Frame write complete. New WrIndex=%d, RdIndex=%d\r\n",
                     Channel->DeviceID, Channel->WrIndex, Channel->RdIndex);
    }
}


/*****************************************************************************/
/*
 * Call back function for write error
 *
 * This callback handles error interrupts for the write channel.
 *
 * @param	CallbackRef is the reference pointer to the channel (VdmaChannel)
 * @param	Mask is the interrupt mask passed from the driver
 *
 * @return	None
 *
 ******************************************************************************/
static void WriteErrorCallBack(void *CallbackRef, u32 Mask)
{
    if (CallbackRef == NULL)
    {
        DEBUG_PRINTF("WriteErrorCallBack: CallbackRef is NULL\r\n");
        return;
    }

    VdmaChannel *Channel = (VdmaChannel *)CallbackRef;

    if (Mask & XAXIVDMA_IXR_ERROR_MASK)
    {
        Channel->WriteError++;

        DEBUG_PRINTF("Channel %d: Write error occurred! Error count=%d\r\n",
                     Channel->DeviceID, Channel->WriteError);

        int Status = XAxiVdma_StartParking(&Channel->Vdma, Channel->WrIndex, XAXIVDMA_WRITE);
        if (Status != XST_SUCCESS)
        {
            DEBUG_PRINTF("WriteErrorCallBack Channel %d: Failed to start parking. Status=%d\r\n",
                         Channel->DeviceID, Status);
            return;
        }
    }
}

int MultiVdmaInit(VdmaChannel *VdmaChannels, int NumChannels)
{
    int Status;
	INTC *IntcInstPtr = &InterruptController;
    for (int i = 0; i < NumChannels; i++)
    {
        XAxiVdma_Config *Config;

        // 閼惧嘲褰嘨DMA闁板秶鐤�
        Config = XAxiVdma_LookupConfig(VdmaChannels[i].DeviceID);
        if (Config == NULL)
        {
            xil_printf("VDMA %d: LookupConfig failed\r\n", i);
            return XST_FAILURE;
        }

        Status = XAxiVdma_CfgInitialize(&VdmaChannels[i].Vdma, Config, Config->BaseAddress);
        if (Status != XST_SUCCESS)
        {
            xil_printf("VDMA %d: CfgInitialize failed\r\n", i);
            return XST_FAILURE;
        }

		vdma_write_stop(&VdmaChannels[i].Vdma);
		XAxiVdma_IntrDisable(&VdmaChannels[i].Vdma, XAXIVDMA_IXR_ALL_MASK, XAXIVDMA_WRITE);

		vdma_write_init(VdmaChannels[i].DeviceID, &VdmaChannels[i].Vdma, VdmaChannels[i].Width * 3, VdmaChannels[i].Height, VdmaChannels[i].Stride * 3,
						VdmaChannels[i].FrameBuffers[0], VdmaChannels[i].FrameBuffers[1], VdmaChannels[i].FrameBuffers[2]);

		XAxiVdma_SetCallBack(&VdmaChannels[i].Vdma, XAXIVDMA_HANDLER_GENERAL,WriteCallBack, (void *)&VdmaChannels[i], XAXIVDMA_WRITE);
		XAxiVdma_SetCallBack(&VdmaChannels[i].Vdma, XAXIVDMA_HANDLER_ERROR,WriteErrorCallBack, (void *)&VdmaChannels[i], XAXIVDMA_WRITE);


		Status = INTC_CONNECT(IntcInstPtr, VdmaChannels[i].S2MM_INTR_ID,
		         (XInterruptHandler)XAxiVdma_WriteIntrHandler, &VdmaChannels[i].Vdma);
		if (Status != XST_SUCCESS)
		{
			xil_printf("Failed write channel connect intc %d\r\n", Status);
			return XST_FAILURE;
		}

		INTC_CONNECT_ENABLE(IntcInstPtr, VdmaChannels[i].S2MM_INTR_ID);

		XAxiVdma_IntrEnable(&VdmaChannels[i].Vdma, XAXIVDMA_IXR_ALL_MASK, XAXIVDMA_WRITE);

        // 閸掓繂顫愰崠鏍拷姘朵壕閻樿埖锟斤拷
        VdmaChannels[i].WrIndex = 0;
        VdmaChannels[i].RdIndex = 0;
        VdmaChannels[i].WriteOneFrameEnd = 0;
        VdmaChannels[i].WriteError = 0;
        VdmaChannels[i].FrameLength = VdmaChannels[i].Width * VdmaChannels[i].Height * 3;
        VdmaChannels[i].pkg_cnt = 1;
        VdmaChannels[i].udp_send_times = (VdmaChannels[i].FrameLength/VdmaChannels[i].UDP_IMG_PACKEG_SIZE);
		vdma_write_start(&VdmaChannels[i].Vdma);

        xil_printf("VDMA %d: Initialized successfully\r\n", i);
    }

    return XST_SUCCESS;
}


int vdma_udp_init(void)
{
    // 閸掓繂顫愰崠鏈MA闁岸浜�
    int Status = MultiVdmaInit(VdmaChannels, NUM_CHANNELS);
    if (Status != XST_SUCCESS)
    {
        xil_printf("VDMA Initialization failed\r\n");
        return XST_FAILURE;
    }

    xil_printf("VDMA Channels initialized and ready.\r\n");
    return Status;
}

#define MAX_ITERATIONS 3 // 濮ｅ繋閲滈柅姘朵壕鏉╂劘顢戦惃鍕付婢堆勵偧閺侊拷
int iterationCounts[NUM_CHANNELS] = {0}; // 濮ｅ繋閲滈柅姘朵壕閻ㄥ嫯顓搁弫鏉挎珤
void MonitorAndExitAfterIterations(void)
{

	for (int i = 0; i < NUM_CHANNELS; i++)
	{
		if (iterationCounts[i] < MAX_ITERATIONS)
		{
			if(VdmaChannels[i].WriteOneFrameEnd >= 0)
			{
				// 閹垫挸宓冭ぐ鎾冲闁岸浜鹃悩鑸碉拷锟�
				xil_printf("Channel %d: WriteEnd=%d, WriteError=%d, WrIndex=%d, RdIndex=%d\r\n",
						VdmaChannels[i].DeviceID, VdmaChannels[i].WriteOneFrameEnd, VdmaChannels[i].WriteError,
						VdmaChannels[i].WrIndex, VdmaChannels[i].RdIndex);

				// 濞撳懘娅庣�瑰本鍨氶弽鍥х箶
				VdmaChannels[i].WriteOneFrameEnd = -1;

				// 婢х偛濮炵拋鈩冩殶閸ｏ拷
				iterationCounts[i]++;

				// 濡拷閺屻儲妲搁崥锕佹彧閸掔増娓舵径褑绻嶇悰灞绢偧閺侊拷
				if (iterationCounts[i] >= MAX_ITERATIONS)
				{
					xil_printf("Channel %d: Reached max iterations (%d), exiting.\r\n",
							VdmaChannels[i].DeviceID, MAX_ITERATIONS);
					return; // 閸忋劑鍎撮柅姘朵壕鐎瑰本鍨氶崥搴拷锟介崙锟�
				}
			}
		}
	}
}
#endif //#if defined (UDP_VIDEO)


u32 vdma_version(XAxiVdma *Vdma) {
	return XAxiVdma_GetVersion(Vdma);
}

int vdma_read_start(XAxiVdma *Vdma) {
	int Status;

	// MM2S Startup
	Status = XAxiVdma_DmaStart(Vdma, XAXIVDMA_READ);
	if (Status != XST_SUCCESS)
	{
	   xil_printf("Start read transfer failed %d\n\r", Status);
	   return XST_FAILURE;
	}

	return XST_SUCCESS;
}


int vdma_read_stop(XAxiVdma *Vdma) {
	XAxiVdma_DmaStop(Vdma, XAXIVDMA_READ);
	return XST_SUCCESS;
}


int vdma_read_init(short DeviceID,short HoriSizeInput,short VertSizeInput,short Stride,unsigned int FrameStoreStartAddr)
{
	XAxiVdma Vdma;
	XAxiVdma_Config *Config;
	XAxiVdma_DmaSetup ReadCfg;
	int Status;


	Config = XAxiVdma_LookupConfig(DeviceID);
	if (NULL == Config) {
		xil_printf("XAxiVdma_LookupConfig failure\r\n");
		return XST_FAILURE;
	}

	Status = XAxiVdma_CfgInitialize(&Vdma, Config, Config->BaseAddress);
	if (Status != XST_SUCCESS) {
		xil_printf("XAxiVdma_CfgInitialize failure\r\n");
		return XST_FAILURE;
	}

	

	ReadCfg.EnableCircularBuf = 1;
	ReadCfg.EnableFrameCounter = 0;
	ReadCfg.FixedFrameStoreAddr = 0;

	ReadCfg.EnableSync = 1;
	ReadCfg.PointNum = 1;

	ReadCfg.FrameDelay = 0;

	ReadCfg.VertSizeInput = VertSizeInput;
	ReadCfg.HoriSizeInput = HoriSizeInput;
	ReadCfg.Stride = Stride;

	Status = XAxiVdma_DmaConfig(&Vdma, XAXIVDMA_READ, &ReadCfg);
	if (Status != XST_SUCCESS) {
			xdbg_printf(XDBG_DEBUG_ERROR,
				"Read channel config failed %d\r\n", Status);

			return XST_FAILURE;
	}


	ReadCfg.FrameStoreStartAddr[0] = FrameStoreStartAddr;

	Status = XAxiVdma_DmaSetBufferAddr(&Vdma, XAXIVDMA_READ, ReadCfg.FrameStoreStartAddr);
	if (Status != XST_SUCCESS) {
			xdbg_printf(XDBG_DEBUG_ERROR,"Read channel set buffer address failed %d\r\n", Status);
			return XST_FAILURE;
	}


	Status = vdma_read_start(&Vdma);
	if (Status != XST_SUCCESS) {
		   xil_printf("error starting VDMA..!");
		   return Status;
	}
	return XST_SUCCESS;

}


int vdma_write_start(XAxiVdma *Vdma) {
	int Status;

	// MM2S Startup
	Status = XAxiVdma_DmaStart(Vdma, XAXIVDMA_WRITE);
	if (Status != XST_SUCCESS)
	{
	   xil_printf("Start write transfer failed %d\n\r", Status);
	   return XST_FAILURE;
	}

	return XST_SUCCESS;
}


int vdma_write_stop(XAxiVdma *Vdma) {
	XAxiVdma_DmaStop(Vdma, XAXIVDMA_WRITE);
	return XST_SUCCESS;
}


int vdma_write_init(short DeviceID,XAxiVdma *Vdma,short HoriSizeInput,short VertSizeInput,short Stride,
		unsigned int FrameStoreStartAddr0,unsigned int FrameStoreStartAddr1,unsigned int FrameStoreStartAddr2)
{
	XAxiVdma_FrameCounter FrameCfg;
	XAxiVdma_DmaSetup WriteCfg;
	int Status;

//	XAxiVdma_Config *Config;
//	Config = XAxiVdma_LookupConfig(DeviceID);
//	if (NULL == Config) {
//		xil_printf("XAxiVdma_LookupConfig failure\r\n");
//		return XST_FAILURE;
//	}
//
//	Status = XAxiVdma_CfgInitialize(Vdma, Config, Config->BaseAddress);
//	if (Status != XST_SUCCESS) {
//		xil_printf("XAxiVdma_CfgInitialize failure\r\n");
//		return XST_FAILURE;
//	}

	XAxiVdma_GetFrameCounter(Vdma, &FrameCfg) ;

	if (FrameCfg.ReadFrameCount == 0)
		FrameCfg.ReadFrameCount = 1 ;

	FrameCfg.WriteFrameCount = 1;
	FrameCfg.WriteDelayTimerCount = 10;
	Status = XAxiVdma_SetFrameCounter(Vdma, &FrameCfg);
	if (Status != XST_SUCCESS) {
		xil_printf(
			"Set frame counter failed %d\r\n", Status);

		if(Status == XST_VDMA_MISMATCH_ERROR)
			xil_printf("DMA Mismatch Error\r\n");

		return XST_FAILURE;
	}

	WriteCfg.EnableCircularBuf = 0;
	WriteCfg.EnableFrameCounter = 0;
	WriteCfg.FixedFrameStoreAddr = 0;

	WriteCfg.EnableSync = 1;
	WriteCfg.PointNum = 1;

	WriteCfg.FrameDelay = 0;

	WriteCfg.VertSizeInput = VertSizeInput;
	WriteCfg.HoriSizeInput = HoriSizeInput;
	WriteCfg.Stride = Stride;

	Status = XAxiVdma_DmaConfig(Vdma, XAXIVDMA_WRITE, &WriteCfg);
	if (Status != XST_SUCCESS) {
			xdbg_printf(XDBG_DEBUG_ERROR,
				"Read channel config failed %d\r\n", Status);

			return XST_FAILURE;
	}


	WriteCfg.FrameStoreStartAddr[0] = FrameStoreStartAddr0;
	WriteCfg.FrameStoreStartAddr[1] = FrameStoreStartAddr1;
	WriteCfg.FrameStoreStartAddr[2] = FrameStoreStartAddr2;

	Status = XAxiVdma_DmaSetBufferAddr(Vdma, XAXIVDMA_WRITE, WriteCfg.FrameStoreStartAddr);
	if (Status != XST_SUCCESS) {
			xdbg_printf(XDBG_DEBUG_ERROR,"Write channel set buffer address failed %d\r\n", Status);
			return XST_FAILURE;
	}


//	Status = vdma_write_start(Vdma);
//	if (Status != XST_SUCCESS) {
//		   xil_printf("error starting VDMA..!");
//		   return Status;
//	}

	return XST_SUCCESS;

}

#if 1

#if (XPAR_XAXIVDMA_NUM_INSTANCES >= 1U)
void vdma_config_0(void)
{
	 /* Start of VDMA Configuration */
	u32 bytePerPixels = 3;
	u32 VDMA_BASE_ADDR = XPAR_AXIVDMA_0_BASEADDR;
	int offset0 = 0; // (y*w+x)*Bpp
	int offset1 = 0; // (y*w+x)*Bpp
	u32 stride0 = 0;
	u32 width0 = 0;
	u32 height0 = 0;
	u32 stride1 = 0;  // crop keeps write Stride
	u32 width1 = 0;
	u32 height1 = 0;

#if (defined R1080P60)
	offset0 = 0; // (y*w+x)*Bpp
	offset1 = 0; // (y*w+x)*Bpp
	stride0 = 1920;
	width0 = 1920;
	height0 = 1080;
	stride1 = 1920;  // crop keeps write Stride
	width1 = 1920;
	height1 = 1080;
#elif (defined R4K30W)
	offset0 = 0; // (y*w+x)*Bpp
	offset1 = 0; // (y*w+x)*Bpp
	stride0 = 3840;
	width0 = 3840;
	height0 = 2160;
	stride1 = 3840;  // crop keeps write Stride
	width1 = 3840;
	height1 = 2160;
#else
	offset0 = 0; // (y*w+x)*Bpp
	offset1 = 0; // (y*w+x)*Bpp
	stride0 = 480;
	width0 = 480;
	height0 = 320;
	stride1 = 480;  // crop keeps write Stride
	width1 = 480;
	height1 = 320;
#endif




#if 1
    /* Configure the Write interface (S2MM)*/
    // S2MM Control Register
    Xil_Out32(VDMA_BASE_ADDR + 0x30, 0x8B);
    //S2MM Start Address 1
    Xil_Out32(VDMA_BASE_ADDR + 0xAC, FRAME_BUFFER_1 + offset0);
    //S2MM Start Address 2
    Xil_Out32(VDMA_BASE_ADDR + 0xB0, FRAME_BUFFER_2 + offset0);
    //S2MM Start Address 3
    Xil_Out32(VDMA_BASE_ADDR + 0xB4, FRAME_BUFFER_3 + offset0);
    //S2MM Frame delay / Stride register
    Xil_Out32(VDMA_BASE_ADDR + 0xA8, stride0*bytePerPixels);
    // S2MM HSIZE register
    Xil_Out32(VDMA_BASE_ADDR + 0xA4, width0*bytePerPixels);
    // S2MM VSIZE register
    Xil_Out32(VDMA_BASE_ADDR + 0xA0, height0);

    /* Configure the Read interface (MM2S)*/
    // MM2S Control Register
    Xil_Out32(VDMA_BASE_ADDR + 0x00, 0x8B);
    // MM2S Start Address 1
    Xil_Out32(VDMA_BASE_ADDR + 0x5C, FRAME_BUFFER_1 + offset1);
    // MM2S Start Address 2
    Xil_Out32(VDMA_BASE_ADDR + 0x60, FRAME_BUFFER_2 + offset1);
    // MM2S Start Address 3
    Xil_Out32(VDMA_BASE_ADDR + 0x64, FRAME_BUFFER_3 + offset1);
    // MM2S Frame delay / Stride register
    Xil_Out32(VDMA_BASE_ADDR + 0x58, stride1*bytePerPixels);
    // MM2S HSIZE register
    Xil_Out32(VDMA_BASE_ADDR + 0x54, width1*bytePerPixels);
    // MM2S VSIZE register
    Xil_Out32(VDMA_BASE_ADDR + 0x50, height1);


    /* End of VDMA Configuration */

#else
    /* Configure the Write interface (S2MM)*/
	// S2MM Control Register
	Xil_Out32(VDMA_BASE_ADDR + 0x30, 0x8B);
	//S2MM Start Address 1
	Xil_Out32(VDMA_BASE_ADDR + 0xAC, FRAME_BUFFER_1 + offset0);
	Xil_Out32(VDMA_BASE_ADDR + 0xB0, 0);
	//S2MM Start Address 2
	Xil_Out32(VDMA_BASE_ADDR + 0xB4, FRAME_BUFFER_2 + offset0);
	Xil_Out32(VDMA_BASE_ADDR + 0xB8, 0);
	//S2MM Start Address 3
	Xil_Out32(VDMA_BASE_ADDR + 0xBC, FRAME_BUFFER_3 + offset0);
	Xil_Out32(VDMA_BASE_ADDR + 0xC0, 0);
	//S2MM Frame delay / Stride register
	Xil_Out32(VDMA_BASE_ADDR + 0xA8, stride0*bytePerPixels);
	// S2MM HSIZE register
	Xil_Out32(VDMA_BASE_ADDR + 0xA4, width0*bytePerPixels);
	// S2MM VSIZE register
	Xil_Out32(VDMA_BASE_ADDR + 0xA0, height0);

	/* Configure the Read interface (MM2S)*/
	// MM2S Control Register
	Xil_Out32(VDMA_BASE_ADDR + 0x00, 0x8B);
	// MM2S Start Address 1
	Xil_Out32(VDMA_BASE_ADDR + 0x5C, FRAME_BUFFER_1 + offset1);
	Xil_Out32(VDMA_BASE_ADDR + 0x60, 0);
	// MM2S Start Address 2
	Xil_Out32(VDMA_BASE_ADDR + 0x64, FRAME_BUFFER_2 + offset1);
	Xil_Out32(VDMA_BASE_ADDR + 0x68, 0);
	// MM2S Start Address 3
	Xil_Out32(VDMA_BASE_ADDR + 0x6C, FRAME_BUFFER_3 + offset1);
	Xil_Out32(VDMA_BASE_ADDR + 0x70, 0);
	// MM2S Frame delay / Stride register
	Xil_Out32(VDMA_BASE_ADDR + 0x58, stride1*bytePerPixels);
	// MM2S HSIZE register
	Xil_Out32(VDMA_BASE_ADDR + 0x54, width1*bytePerPixels);
	// MM2S VSIZE register
	Xil_Out32(VDMA_BASE_ADDR + 0x50, height1);


	/* End of VDMA Configuration */

#endif

    xil_printf("VDMA_0 started!\r\n");
}
#endif

#if (XPAR_XAXIVDMA_NUM_INSTANCES >= 2U)
void vdma_config_1(void)
{
	 /* Start of VDMA Configuration */
	u32 bytePerPixels = 3;
	u32 VDMA_BASE_ADDR = XPAR_AXIVDMA_1_BASEADDR;
	int offset0 = 0; // (y*w+x)*Bpp
	int offset1 = 0; // (y*w+x)*Bpp
	u32 stride0 = 0;
	u32 width0 = 0;
	u32 height0 = 0;
	u32 stride1 = 0;  // crop keeps write Stride
	u32 width1 = 0;
	u32 height1 = 0;

#if (defined R1080P60)
	offset0 = 0; // (y*w+x)*Bpp
	offset1 = 0; // (y*w+x)*Bpp
	stride0 = 1920;
	width0 = 1920;
	height0 = 1080;
	stride1 = 1920;  // crop keeps write Stride
	width1 = 1920;
	height1 = 1080;
#elif (defined R4K30W)
//	offset0 = 0; // (y*w+x)*Bpp
//	offset1 = 0; // (y*w+x)*Bpp
//	stride0 = 3840;
//	width0 = 3840;
//	height0 = 2160;
//	stride1 = 3840;  // crop keeps write Stride
//	width1 = 3840;
//	height1 = 2160;

//	offset0 = 0; // (y*w+x)*Bpp
//	offset1 = 0; // (y*w+x)*Bpp
//	stride0 = 1920;
//	width0 = 1920;
//	height0 = 1286;
//	stride1 = 1920;  // crop keeps write Stride
//	width1 = 1920;
//	height1 = 1286;

	offset0 = 0; // (y*w+x)*Bpp
	offset1 = 0; // (y*w+x)*Bpp
	stride0 = 1920;
	width0 = 1920;
	height0 = 1551;
	stride1 = 1920;  // crop keeps write Stride
	width1 = 1920;
	height1 = 1551;

#else
	offset0 = 0; // (y*w+x)*Bpp
	offset1 = 0; // (y*w+x)*Bpp
	stride0 = 480;
	width0 = 480;
	height0 = 320;
	stride1 = 480;  // crop keeps write Stride
	width1 = 480;
	height1 = 320;
#endif


#if 1
    /* Configure the Write interface (S2MM)*/
    // S2MM Control Register
    Xil_Out32(VDMA_BASE_ADDR + 0x30, 0x8B);
    //S2MM Start Address 1
    Xil_Out32(VDMA_BASE_ADDR + 0xAC, FRAME_BUFFER_4 + offset0);
    //S2MM Start Address 2
    Xil_Out32(VDMA_BASE_ADDR + 0xB0, FRAME_BUFFER_5 + offset0);
    //S2MM Start Address 3
    Xil_Out32(VDMA_BASE_ADDR + 0xB4, FRAME_BUFFER_6 + offset0);
    //S2MM Frame delay / Stride register
    Xil_Out32(VDMA_BASE_ADDR + 0xA8, stride0*bytePerPixels);
    // S2MM HSIZE register
    Xil_Out32(VDMA_BASE_ADDR + 0xA4, width0*bytePerPixels);
    // S2MM VSIZE register
    Xil_Out32(VDMA_BASE_ADDR + 0xA0, height0);

    /* Configure the Read interface (MM2S)*/
    // MM2S Control Register
    Xil_Out32(VDMA_BASE_ADDR + 0x00, 0x8B);
    // MM2S Start Address 1
    Xil_Out32(VDMA_BASE_ADDR + 0x5C, FRAME_BUFFER_4 + offset1);
    // MM2S Start Address 2
    Xil_Out32(VDMA_BASE_ADDR + 0x60, FRAME_BUFFER_5 + offset1);
    // MM2S Start Address 3
    Xil_Out32(VDMA_BASE_ADDR + 0x64, FRAME_BUFFER_6 + offset1);

    // MM2S Frame delay / Stride register
    Xil_Out32(VDMA_BASE_ADDR + 0x58, stride1*bytePerPixels);
    // MM2S HSIZE register
    Xil_Out32(VDMA_BASE_ADDR + 0x54, width1*bytePerPixels);
    // MM2S VSIZE register
    Xil_Out32(VDMA_BASE_ADDR + 0x50, height1);


    /* End of VDMA Configuration */

#else
    /* Configure the Write interface (S2MM)*/
	// S2MM Control Register
	Xil_Out32(VDMA_BASE_ADDR + 0x30, 0x8B);
	//S2MM Start Address 1
	Xil_Out32(VDMA_BASE_ADDR + 0xAC, FRAME_BUFFER_4 + offset0);
	Xil_Out32(VDMA_BASE_ADDR + 0xB0, 0);
	//S2MM Start Address 2
	Xil_Out32(VDMA_BASE_ADDR + 0xB4, FRAME_BUFFER_5 + offset0);
	Xil_Out32(VDMA_BASE_ADDR + 0xB8, 0);
	//S2MM Start Address 3
	Xil_Out32(VDMA_BASE_ADDR + 0xBC, FRAME_BUFFER_6 + offset0);
	Xil_Out32(VDMA_BASE_ADDR + 0xC0, 0);
	//S2MM Frame delay / Stride register
	Xil_Out32(VDMA_BASE_ADDR + 0xA8, stride0*bytePerPixels);
	// S2MM HSIZE register
	Xil_Out32(VDMA_BASE_ADDR + 0xA4, width0*bytePerPixels);
	// S2MM VSIZE register
	Xil_Out32(VDMA_BASE_ADDR + 0xA0, height0);

	/* Configure the Read interface (MM2S)*/
	// MM2S Control Register
	Xil_Out32(VDMA_BASE_ADDR + 0x00, 0x8B);
	// MM2S Start Address 1
	Xil_Out32(VDMA_BASE_ADDR + 0x5C, FRAME_BUFFER_4 + offset1);
	Xil_Out32(VDMA_BASE_ADDR + 0x60, 0);
	// MM2S Start Address 2
	Xil_Out32(VDMA_BASE_ADDR + 0x64, FRAME_BUFFER_5 + offset1);
	Xil_Out32(VDMA_BASE_ADDR + 0x68, 0);
	// MM2S Start Address 3
	Xil_Out32(VDMA_BASE_ADDR + 0x6C, FRAME_BUFFER_6 + offset1);
	Xil_Out32(VDMA_BASE_ADDR + 0x70, 0);
	// MM2S Frame delay / Stride register
	Xil_Out32(VDMA_BASE_ADDR + 0x58, stride1*bytePerPixels);
	// MM2S HSIZE register
	Xil_Out32(VDMA_BASE_ADDR + 0x54, width1*bytePerPixels);
	// MM2S VSIZE register
	Xil_Out32(VDMA_BASE_ADDR + 0x50, height1);


	/* End of VDMA Configuration */

#endif
	xil_printf("VDMA_1 started!\r\n");
}
#endif

#if (XPAR_XAXIVDMA_NUM_INSTANCES >= 3U)
void vdma_config_2(void)
{
	 /* Start of VDMA Configuration */
	u32 bytePerPixels = 3;
	u32 VDMA_BASE_ADDR = XPAR_AXIVDMA_2_BASEADDR;
	int offset0 = 0; // (y*w+x)*Bpp
	int offset1 = 0; // (y*w+x)*Bpp
	u32 stride0 = 0;
	u32 width0 = 0;
	u32 height0 = 0;
	u32 stride1 = 0;  // crop keeps write Stride
	u32 width1 = 0;
	u32 height1 = 0;

#if (defined R1080P60)
	offset0 = 0; // (y*w+x)*Bpp
	offset1 = 0; // (y*w+x)*Bpp
	stride0 = 1920;
	width0 = 1920;
	height0 = 1080;
	stride1 = 1920;  // crop keeps write Stride
	width1 = 1920;
	height1 = 1080;
#elif (defined R4K30W)
	//	offset0 = 0; // (y*w+x)*Bpp
	//	offset1 = 0; // (y*w+x)*Bpp
	//	stride0 = 3840;
	//	width0 = 3840;
	//	height0 = 2160;
	//	stride1 = 3840;  // crop keeps write Stride
	//	width1 = 3840;
	//	height1 = 2160;

	offset0 = 0; // (y*w+x)*Bpp
	offset1 = 0; // (y*w+x)*Bpp
	stride0 = 1920;
	width0 = 1920;
	height0 = 1551;
	stride1 = 1920;  // crop keeps write Stride
	width1 = 1920;
	height1 = 1551;
#else
	offset0 = 0; // (y*w+x)*Bpp
	offset1 = 0; // (y*w+x)*Bpp
	stride0 = 480;
	width0 = 480;
	height0 = 320;
	stride1 = 480;  // crop keeps write Stride
	width1 = 480;
	height1 = 320;
#endif


#if 1
    /* Configure the Write interface (S2MM)*/
    // S2MM Control Register
    Xil_Out32(VDMA_BASE_ADDR + 0x30, 0x8B);
    //S2MM Start Address 1
    Xil_Out32(VDMA_BASE_ADDR + 0xAC, FRAME_BUFFER_7 + offset0);
    //S2MM Start Address 2
    Xil_Out32(VDMA_BASE_ADDR + 0xB0, FRAME_BUFFER_8 + offset0);
    //S2MM Start Address 3
    Xil_Out32(VDMA_BASE_ADDR + 0xB4, FRAME_BUFFER_9 + offset0);
    //S2MM Frame delay / Stride register
    Xil_Out32(VDMA_BASE_ADDR + 0xA8, stride0*bytePerPixels);
    // S2MM HSIZE register
    Xil_Out32(VDMA_BASE_ADDR + 0xA4, width0*bytePerPixels);
    // S2MM VSIZE register
    Xil_Out32(VDMA_BASE_ADDR + 0xA0, height0);

    /* Configure the Read interface (MM2S)*/
    // MM2S Control Register
    Xil_Out32(VDMA_BASE_ADDR + 0x00, 0x8B);
    // MM2S Start Address 1
    Xil_Out32(VDMA_BASE_ADDR + 0x5C, FRAME_BUFFER_7 + offset1);
    // MM2S Start Address 2
    Xil_Out32(VDMA_BASE_ADDR + 0x60, FRAME_BUFFER_8 + offset1);
    // MM2S Start Address 3
    Xil_Out32(VDMA_BASE_ADDR + 0x64, FRAME_BUFFER_9 + offset1);
    // MM2S Frame delay / Stride register
    Xil_Out32(VDMA_BASE_ADDR + 0x58, stride1*bytePerPixels);
    // MM2S HSIZE register
    Xil_Out32(VDMA_BASE_ADDR + 0x54, width1*bytePerPixels);
    // MM2S VSIZE register
    Xil_Out32(VDMA_BASE_ADDR + 0x50, height1);


    /* End of VDMA Configuration */

#else
    /* Configure the Write interface (S2MM)*/
	// S2MM Control Register
	Xil_Out32(VDMA_BASE_ADDR + 0x30, 0x8B);
	//S2MM Start Address 1
	Xil_Out32(VDMA_BASE_ADDR + 0xAC, FRAME_BUFFER_7 + offset0);
	Xil_Out32(VDMA_BASE_ADDR + 0xB0, 0);
	//S2MM Start Address 2
	Xil_Out32(VDMA_BASE_ADDR + 0xB4, FRAME_BUFFER_8 + offset0);
	Xil_Out32(VDMA_BASE_ADDR + 0xB8, 0);
	//S2MM Start Address 3
	Xil_Out32(VDMA_BASE_ADDR + 0xBC, FRAME_BUFFER_9 + offset0);
	Xil_Out32(VDMA_BASE_ADDR + 0xC0, 0);
	//S2MM Frame delay / Stride register
	Xil_Out32(VDMA_BASE_ADDR + 0xA8, stride0*bytePerPixels);
	// S2MM HSIZE register
	Xil_Out32(VDMA_BASE_ADDR + 0xA4, width0*bytePerPixels);
	// S2MM VSIZE register
	Xil_Out32(VDMA_BASE_ADDR + 0xA0, height0);

	/* Configure the Read interface (MM2S)*/
	// MM2S Control Register
	Xil_Out32(VDMA_BASE_ADDR + 0x00, 0x8B);
	// MM2S Start Address 1
	Xil_Out32(VDMA_BASE_ADDR + 0x5C, FRAME_BUFFER_7 + offset1);
	Xil_Out32(VDMA_BASE_ADDR + 0x60, 0);
	// MM2S Start Address 2
	Xil_Out32(VDMA_BASE_ADDR + 0x64, FRAME_BUFFER_8 + offset1);
	Xil_Out32(VDMA_BASE_ADDR + 0x68, 0);
	// MM2S Start Address 3
	Xil_Out32(VDMA_BASE_ADDR + 0x6C, FRAME_BUFFER_9 + offset1);
	Xil_Out32(VDMA_BASE_ADDR + 0x70, 0);
	// MM2S Frame delay / Stride register
	Xil_Out32(VDMA_BASE_ADDR + 0x58, stride1*bytePerPixels);
	// MM2S HSIZE register
	Xil_Out32(VDMA_BASE_ADDR + 0x54, width1*bytePerPixels);
	// MM2S VSIZE register
	Xil_Out32(VDMA_BASE_ADDR + 0x50, height1);


	/* End of VDMA Configuration */

#endif
	xil_printf("VDMA_2 started!\r\n");
}
#endif

#if (XPAR_XAXIVDMA_NUM_INSTANCES >= 4U)
void vdma_config_3(void)
{
	 /* Start of VDMA Configuration */
	u32 bytePerPixels = 3;
	u32 VDMA_BASE_ADDR = XPAR_AXIVDMA_6_BASEADDR;
	int offset0 = 0; // (y*w+x)*Bpp
	int offset1 = 0; // (y*w+x)*Bpp
	u32 stride0 = 0;
	u32 width0 = 0;
	u32 height0 = 0;
	u32 stride1 = 0;  // crop keeps write Stride
	u32 width1 = 0;
	u32 height1 = 0;

#if (defined R1080P60)
	offset0 = 0; // (y*w+x)*Bpp
	offset1 = 0; // (y*w+x)*Bpp
	stride0 = 1920;
	width0 = 1920;
	height0 = 1080;
	stride1 = 1920;  // crop keeps write Stride
	width1 = 1920;
	height1 = 1080;
#elif (defined R4K30W)
	offset0 = 0; // (y*w+x)*Bpp
	offset1 = 0; // (y*w+x)*Bpp
	stride0 = 3840;
	width0 = 3840;
	height0 = 2160;
	stride1 = 3840;  // crop keeps write Stride
	width1 = 3840;
	height1 = 2160;
#else
	offset0 = 0; // (y*w+x)*Bpp
	offset1 = 0; // (y*w+x)*Bpp
	stride0 = 480;
	width0 = 480;
	height0 = 320;
	stride1 = 480;  // crop keeps write Stride
	width1 = 480;
	height1 = 320;
#endif


#if 1
    /* Configure the Write interface (S2MM)*/
    // S2MM Control Register
    Xil_Out32(VDMA_BASE_ADDR + 0x30, 0x8B);
    //S2MM Start Address 1
    Xil_Out32(VDMA_BASE_ADDR + 0xAC, FRAME_BUFFER_10 + offset0);
    //S2MM Start Address 2
    Xil_Out32(VDMA_BASE_ADDR + 0xB0, FRAME_BUFFER_11 + offset0);
    //S2MM Start Address 3
    Xil_Out32(VDMA_BASE_ADDR + 0xB4, FRAME_BUFFER_12 + offset0);
    //S2MM Frame delay / Stride register
    Xil_Out32(VDMA_BASE_ADDR + 0xA8, stride0*bytePerPixels);
    // S2MM HSIZE register
    Xil_Out32(VDMA_BASE_ADDR + 0xA4, width0*bytePerPixels);
    // S2MM VSIZE register
    Xil_Out32(VDMA_BASE_ADDR + 0xA0, height0);

    /* Configure the Read interface (MM2S)*/
    // MM2S Control Register
    Xil_Out32(VDMA_BASE_ADDR + 0x00, 0x8B);
    // MM2S Start Address 1
    Xil_Out32(VDMA_BASE_ADDR + 0x5C, FRAME_BUFFER_10 + offset1);
    // MM2S Start Address 2
    Xil_Out32(VDMA_BASE_ADDR + 0x60, FRAME_BUFFER_11 + offset1);
    // MM2S Start Address 3
    Xil_Out32(VDMA_BASE_ADDR + 0x64, FRAME_BUFFER_12 + offset1);
    // MM2S Frame delay / Stride register
    Xil_Out32(VDMA_BASE_ADDR + 0x58, stride1*bytePerPixels);
    // MM2S HSIZE register
    Xil_Out32(VDMA_BASE_ADDR + 0x54, width1*bytePerPixels);
    // MM2S VSIZE register
    Xil_Out32(VDMA_BASE_ADDR + 0x50, height1);


    /* End of VDMA Configuration */

#else
    /* Configure the Write interface (S2MM)*/
	// S2MM Control Register
	Xil_Out32(VDMA_BASE_ADDR + 0x30, 0x8B);
	//S2MM Start Address 1
	Xil_Out32(VDMA_BASE_ADDR + 0xAC, FRAME_BUFFER_10 + offset0);
	Xil_Out32(VDMA_BASE_ADDR + 0xB0, 0);
	//S2MM Start Address 2
	Xil_Out32(VDMA_BASE_ADDR + 0xB4, FRAME_BUFFER_11 + offset0);
	Xil_Out32(VDMA_BASE_ADDR + 0xB8, 0);
	//S2MM Start Address 3
	Xil_Out32(VDMA_BASE_ADDR + 0xBC, FRAME_BUFFER_12 + offset0);
	Xil_Out32(VDMA_BASE_ADDR + 0xC0, 0);
	//S2MM Frame delay / Stride register
	Xil_Out32(VDMA_BASE_ADDR + 0xA8, stride0*bytePerPixels);
	// S2MM HSIZE register
	Xil_Out32(VDMA_BASE_ADDR + 0xA4, width0*bytePerPixels);
	// S2MM VSIZE register
	Xil_Out32(VDMA_BASE_ADDR + 0xA0, height0);

	/* Configure the Read interface (MM2S)*/
	// MM2S Control Register
	Xil_Out32(VDMA_BASE_ADDR + 0x00, 0x8B);
	// MM2S Start Address 1
	Xil_Out32(VDMA_BASE_ADDR + 0x5C, FRAME_BUFFER_10 + offset1);
	Xil_Out32(VDMA_BASE_ADDR + 0x60, 0);
	// MM2S Start Address 2
	Xil_Out32(VDMA_BASE_ADDR + 0x64, FRAME_BUFFER_11 + offset1);
	Xil_Out32(VDMA_BASE_ADDR + 0x68, 0);
	// MM2S Start Address 3
	Xil_Out32(VDMA_BASE_ADDR + 0x6C, FRAME_BUFFER_12 + offset1);
	Xil_Out32(VDMA_BASE_ADDR + 0x70, 0);
	// MM2S Frame delay / Stride register
	Xil_Out32(VDMA_BASE_ADDR + 0x58, stride1*bytePerPixels);
	// MM2S HSIZE register
	Xil_Out32(VDMA_BASE_ADDR + 0x54, width1*bytePerPixels);
	// MM2S VSIZE register
	Xil_Out32(VDMA_BASE_ADDR + 0x50, height1);


	/* End of VDMA Configuration */

#endif
	xil_printf("VDMA_3 started!\r\n");
}
#endif

#if (XPAR_XAXIVDMA_NUM_INSTANCES >= 5U)
void vdma_config_4(void)
{
	 /* Start of VDMA Configuration */
	u32 bytePerPixels = 3;
	u32 VDMA_BASE_ADDR = VDMA_BASE_ADDR;
	int offset0 = 0; // (y*w+x)*Bpp
	int offset1 = 0; // (y*w+x)*Bpp
	u32 stride0 = 0;
	u32 width0 = 0;
	u32 height0 = 0;
	u32 stride1 = 0;  // crop keeps write Stride
	u32 width1 = 0;
	u32 height1 = 0;

#if (defined R1080P60)
	offset0 = 0; // (y*w+x)*Bpp
	offset1 = 0; // (y*w+x)*Bpp
	stride0 = 1920;
	width0 = 1920;
	height0 = 1080;
	stride1 = 1920;  // crop keeps write Stride
	width1 = 1920;
	height1 = 1080;
#elif (defined R4K30W)
	offset0 = 0; // (y*w+x)*Bpp
	offset1 = 0; // (y*w+x)*Bpp
	stride0 = 3840;
	width0 = 3840;
	height0 = 2160;
	stride1 = 3840;  // crop keeps write Stride
	width1 = 3840;
	height1 = 2160;
#else
	offset0 = 0; // (y*w+x)*Bpp
	offset1 = 0; // (y*w+x)*Bpp
	stride0 = 480;
	width0 = 480;
	height0 = 320;
	stride1 = 480;  // crop keeps write Stride
	width1 = 480;
	height1 = 320;
#endif


#if 1
    /* Configure the Write interface (S2MM)*/
    // S2MM Control Register
    Xil_Out32(VDMA_BASE_ADDR + 0x30, 0x8B);
    //S2MM Start Address 1
    Xil_Out32(VDMA_BASE_ADDR + 0xAC, FRAME_BUFFER_13 + offset0);
    //S2MM Start Address 2
    Xil_Out32(VDMA_BASE_ADDR + 0xB0, FRAME_BUFFER_14 + offset0);
    //S2MM Start Address 3
    Xil_Out32(VDMA_BASE_ADDR + 0xB4, FRAME_BUFFER_15 + offset0);
    //S2MM Frame delay / Stride register
    Xil_Out32(VDMA_BASE_ADDR + 0xA8, stride0*bytePerPixels);
    // S2MM HSIZE register
    Xil_Out32(VDMA_BASE_ADDR + 0xA4, width0*bytePerPixels);
    // S2MM VSIZE register
    Xil_Out32(VDMA_BASE_ADDR + 0xA0, height0);

    /* Configure the Read interface (MM2S)*/
    // MM2S Control Register
    Xil_Out32(VDMA_BASE_ADDR + 0x00, 0x8B);
    // MM2S Start Address 1
    Xil_Out32(VDMA_BASE_ADDR + 0x5C, FRAME_BUFFER_13 + offset1);
    // MM2S Start Address 2
    Xil_Out32(VDMA_BASE_ADDR + 0x60, FRAME_BUFFER_14 + offset1);
    // MM2S Start Address 3
    Xil_Out32(VDMA_BASE_ADDR + 0x64, FRAME_BUFFER_15 + offset1);
    // MM2S Frame delay / Stride register
    Xil_Out32(VDMA_BASE_ADDR + 0x58, stride1*bytePerPixels);
    // MM2S HSIZE register
    Xil_Out32(VDMA_BASE_ADDR + 0x54, width1*bytePerPixels);
    // MM2S VSIZE register
    Xil_Out32(VDMA_BASE_ADDR + 0x50, height1);


    /* End of VDMA Configuration */

#else
    /* Configure the Write interface (S2MM)*/
	// S2MM Control Register
	Xil_Out32(VDMA_BASE_ADDR + 0x30, 0x8B);
	//S2MM Start Address 1
	Xil_Out32(VDMA_BASE_ADDR + 0xAC, FRAME_BUFFER_13 + offset0);
	Xil_Out32(VDMA_BASE_ADDR + 0xB0, 0);
	//S2MM Start Address 2
	Xil_Out32(VDMA_BASE_ADDR + 0xB4, FRAME_BUFFER_14 + offset0);
	Xil_Out32(VDMA_BASE_ADDR + 0xB8, 0);
	//S2MM Start Address 3
	Xil_Out32(VDMA_BASE_ADDR + 0xBC, FRAME_BUFFER_15 + offset0);
	Xil_Out32(VDMA_BASE_ADDR + 0xC0, 0);
	//S2MM Frame delay / Stride register
	Xil_Out32(VDMA_BASE_ADDR + 0xA8, stride0*bytePerPixels);
	// S2MM HSIZE register
	Xil_Out32(VDMA_BASE_ADDR + 0xA4, width0*bytePerPixels);
	// S2MM VSIZE register
	Xil_Out32(VDMA_BASE_ADDR + 0xA0, height0);

	/* Configure the Read interface (MM2S)*/
	// MM2S Control Register
	Xil_Out32(VDMA_BASE_ADDR + 0x00, 0x8B);
	// MM2S Start Address 1
	Xil_Out32(VDMA_BASE_ADDR + 0x5C, FRAME_BUFFER_13 + offset1);
	Xil_Out32(VDMA_BASE_ADDR + 0x60, 0);
	// MM2S Start Address 2
	Xil_Out32(VDMA_BASE_ADDR + 0x64, FRAME_BUFFER_14 + offset1);
	Xil_Out32(VDMA_BASE_ADDR + 0x68, 0);
	// MM2S Start Address 3
	Xil_Out32(VDMA_BASE_ADDR + 0x6C, FRAME_BUFFER_15 + offset1);
	Xil_Out32(VDMA_BASE_ADDR + 0x70, 0);
	// MM2S Frame delay / Stride register
	Xil_Out32(VDMA_BASE_ADDR + 0x58, stride1*bytePerPixels);
	// MM2S HSIZE register
	Xil_Out32(VDMA_BASE_ADDR + 0x54, width1*bytePerPixels);
	// MM2S VSIZE register
	Xil_Out32(VDMA_BASE_ADDR + 0x50, height1);


	/* End of VDMA Configuration */

#endif
	xil_printf("VDMA_4 started!\r\n");
}
#endif



void vdma_config(void)
{
#if (XPAR_XAXIVDMA_NUM_INSTANCES >= 1U)
	vdma_config_0();
#endif
#if (XPAR_XAXIVDMA_NUM_INSTANCES >= 2U)
	vdma_config_1();
#endif
#if (XPAR_XAXIVDMA_NUM_INSTANCES >= 3U)
	vdma_config_2();
#endif
#if (XPAR_XAXIVDMA_NUM_INSTANCES >= 4U)
	vdma_config_3();
#endif
#if (XPAR_XAXIVDMA_NUM_INSTANCES >= 5U)
//	vdma_config_4();
#endif
}

#if (XPAR_XAXIVDMA_NUM_INSTANCES >= 1U)
void clear_vdma_0(void)
{
	u32 bytePerPixels = 3;
	u32 VDMA_BASE_ADDR = XPAR_AXIVDMA_0_BASEADDR;
	u32 line = 0;
	u32 column = 0;

#if (defined R1080P60)
	line = 1920;
	column = 1080;
#elif (defined R4K30W)
	line = 3840;
	column = 2160;
#else
	line = 3840;
	column = 2160;
#endif

    Xil_Out32(VDMA_BASE_ADDR + 0x00, 0x8A);//stop mm2s
	Xil_Out32(VDMA_BASE_ADDR + 0x30, 0x8A);//stop s2mm

	Xil_DCacheDisable();
    memset(FRAME_BUFFER_1,0xff,line*column*bytePerPixels);//background
    memset(FRAME_BUFFER_2,0xff,line*column*bytePerPixels);//background
    memset(FRAME_BUFFER_3,0xff,line*column*bytePerPixels);//background

	Xil_DCacheEnable();

	xil_printf("clear vdma_0 Done\n\r");
	vdma_config_0();
}
#endif

#if (XPAR_XAXIVDMA_NUM_INSTANCES >= 2U)
void clear_vdma_1(void)
{
	u32 bytePerPixels = 3;
	u32 VDMA_BASE_ADDR = XPAR_AXIVDMA_1_BASEADDR;
	u32 line = 0;
	u32 column = 0;

#if (defined R1080P60)
	line = 1920;
	column = 1080;
#elif (defined R4K30W)
	line = 3840;
	column = 2160;
#else
	line = 3840;
	column = 2160;
#endif

    Xil_Out32(VDMA_BASE_ADDR + 0x00, 0x8A);//stop mm2s
	Xil_Out32(VDMA_BASE_ADDR + 0x30, 0x8A);//stop s2mm

	Xil_DCacheDisable();
    memset(FRAME_BUFFER_4,0xff,line*column*bytePerPixels);//background
    memset(FRAME_BUFFER_5,0xff,line*column*bytePerPixels);//background
    memset(FRAME_BUFFER_6,0xff,line*column*bytePerPixels);//background
	Xil_DCacheEnable();

	xil_printf("clear vdma_1 Done\n\r");
	vdma_config_1();
}
#endif

#if (XPAR_XAXIVDMA_NUM_INSTANCES >= 3U)
void clear_vdma_2(void)
{
	u32 bytePerPixels = 3;
	u32 VDMA_BASE_ADDR = XPAR_AXIVDMA_2_BASEADDR;
	u32 line = 0;
	u32 column = 0;

#if (defined R1080P60)
	line = 1920;
	column = 1080;
#elif (defined R4K30W)
	line = 3840;
	column = 2160;
#else
	line = 1920;
	column = 1280;
#endif

    Xil_Out32(VDMA_BASE_ADDR + 0x00, 0x8A);//stop mm2s
	Xil_Out32(VDMA_BASE_ADDR + 0x30, 0x8A);//stop s2mm

	Xil_DCacheDisable();
    memset(FRAME_BUFFER_7,0xff,line*column*bytePerPixels);//background
    memset(FRAME_BUFFER_8,0xff,line*column*bytePerPixels);//background
    memset(FRAME_BUFFER_9,0xff,line*column*bytePerPixels);//background
	Xil_DCacheEnable();

	xil_printf("clear vdma_2 Done\n\r");
	vdma_config_2();

}
#endif

#if (XPAR_XAXIVDMA_NUM_INSTANCES >= 4U)
void clear_vdma_3(void)
{
	u32 bytePerPixels = 3;
	u32 VDMA_BASE_ADDR = XPAR_AXIVDMA_6_BASEADDR;
	u32 line = 0;
	u32 column = 0;

#if (defined R1080P60)
	line = 1920;
	column = 1080;
#elif (defined R4K30W)
	line = 3840;
	column = 2160;
#else
	line = 1920;
	column = 1280;
#endif

	u32 i=0,j=0;
    UINTPTR Addr1=FRAME_BUFFER_1,Addr2=FRAME_BUFFER_2,Addr3=FRAME_BUFFER_3;
    UINTPTR Addr4=FRAME_BUFFER_4,Addr5=FRAME_BUFFER_5,Addr6=FRAME_BUFFER_6;
    UINTPTR Addr7=FRAME_BUFFER_7,Addr8=FRAME_BUFFER_8,Addr9=FRAME_BUFFER_9;
    UINTPTR Addr10=FRAME_BUFFER_10,Addr11=FRAME_BUFFER_11,Addr12=FRAME_BUFFER_12;

    Xil_Out32(VDMA_BASE_ADDR + 0x00, 0x8A);//stop mm2s
	Xil_Out32(VDMA_BASE_ADDR + 0x30, 0x8A);//stop s2mm

	Xil_DCacheDisable();

    memset(FRAME_BUFFER_10,0xff,line*column*bytePerPixels);//background
    memset(FRAME_BUFFER_11,0xff,line*column*bytePerPixels);//background
    memset(FRAME_BUFFER_12,0xff,line*column*bytePerPixels);//background

    Xil_DCacheEnable();
    xil_printf("clear vdma_3 Done\n\r");
}
#endif

#if (XPAR_XAXIVDMA_NUM_INSTANCES >= 5U)
void clear_vdma_4(void)
{
	u32 bytePerPixels = 3;
	u32 VDMA_BASE_ADDR = XPAR_AXIVDMA_4_BASEADDR;
	u32 line = 0;
	u32 column = 0;

#if (defined R1080P60)
	line = 1920;
	column = 1080;
#elif (defined R4K30W)
	line = 3840;
	column = 2160;
#else
	line = 1920;
	column = 1280;
#endif

	u32 i=0,j=0;
    UINTPTR Addr1=FRAME_BUFFER_1,Addr2=FRAME_BUFFER_2,Addr3=FRAME_BUFFER_3;
    UINTPTR Addr4=FRAME_BUFFER_4,Addr5=FRAME_BUFFER_5,Addr6=FRAME_BUFFER_6;
    UINTPTR Addr7=FRAME_BUFFER_7,Addr8=FRAME_BUFFER_8,Addr9=FRAME_BUFFER_9;
    UINTPTR Addr10=FRAME_BUFFER_10,Addr11=FRAME_BUFFER_11,Addr12=FRAME_BUFFER_12;
    UINTPTR Addr13=FRAME_BUFFER_13,Addr14=FRAME_BUFFER_14,Addr15=FRAME_BUFFER_15;

    Xil_Out32(VDMA_BASE_ADDR + 0x00, 0x8A);//stop mm2s
	Xil_Out32(VDMA_BASE_ADDR + 0x30, 0x8A);//stop s2mm

	Xil_DCacheDisable();

#if 0
	for(i=0; i < column; i++)
	{
		for(j=0; j < line*bytePerPixels/8; j++)
		{
			Xil_Out64(Addr10, 0xFFFFFFFFFFFFFFFF);
			Xil_Out64(Addr11, 0xFFFFFFFFFFFFFFFF);
			Xil_Out64(Addr12, 0xFFFFFFFFFFFFFFFF);

			Addr10+=8;
			Addr11+=8;
			Addr12+=8;

		}
	}
#else
    memset(FRAME_BUFFER_13,0xff,line*column*bytePerPixels);//background
    memset(FRAME_BUFFER_14,0xff,line*column*bytePerPixels);//background
    memset(FRAME_BUFFER_15,0xff,line*column*bytePerPixels);//background

#endif
    Xil_DCacheEnable();
    xil_printf("clear vdma_4 Done\n\r");
}

#endif

void clear_display(void)
{
#if (XPAR_XAXIVDMA_NUM_INSTANCES >= 1U)
	clear_vdma_0();
#endif
#if (XPAR_XAXIVDMA_NUM_INSTANCES >= 2U)
	clear_vdma_1();
#endif
#if (XPAR_XAXIVDMA_NUM_INSTANCES >= 3U)
	clear_vdma_2();
#endif
#if (XPAR_XAXIVDMA_NUM_INSTANCES >= 4U)
	clear_vdma_3();
#endif
#if (XPAR_XAXIVDMA_NUM_INSTANCES >= 5U)
//	clear_vdma_4();
#endif

	xil_printf("clear Done\n\r");

}

#endif

#endif
